/*
* cpu.c
*
* Created: 1/18/2020 9:04:01 PM
*/

#include <avr/io.h>
#include <avr/cpufunc.h>
#include <avr/pgmspace.h>

#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>

#include "Common.h"
#include "tests.h"

static bool WaitAccess()
{
	for( int i = 10000; (PING & 0b00100000) != 0; i-- )
	{
		if( i == 0 )
		{
			printf_P( PSTR( "Processor not making I/O request\n" ) );
			return false;
		}
	}

	while( (PINH & 0b00010100) == 0b00010100 );

	return true;
}

bool WaitNextAccess()
{
	// Strobe the WAIT generator
	PORTB &= 0b11111110;
	PORTB |= 0b00000001;

	// Stop any data output
	DDRD = 0b00000000;

	for( uint8_t i = 0xff; (PING & 0b00100000) != 0; i-- )
	{
		if( i == 0 )
		{
			printf_P( PSTR( "No I/O detected from CPU\n" ) );
			return false;
		}
	}

	while( (PINH & 0b00010100) == 0b00010100 );

	return true;
}

bool CPUTest()
{
	printf_P( PSTR( "Beginning CPU test\n" ) );
	
	bool pass = true;
	
	typedef enum { _m1, _m1_halt, _halt, _int, _nmi, _noint, _intack, _mrd, _mwr, _ird, _iwr, _text, _done } state_m;
	typedef struct {
		uint16_t addr;
		state_m state;
		uint8_t data;
		char const *info;
	} cpu_script_t;
	cpu_script_t script_entry;
	cpu_script_t const *tidx;
	static char const s0[] PROGMEM = "Testing address lines";
	static char const s1[] PROGMEM = "JR 0002";
	static char const s2[] PROGMEM = "JR 0004";
	static char const s3[] PROGMEM = "JR 0008";
	static char const s4[] PROGMEM = "JR 0010";
	static char const s5[] PROGMEM = "JR 0020";
	static char const s6[] PROGMEM = "JR 0040";
	static char const s7[] PROGMEM = "JR 0080";
	static char const s8[] PROGMEM = "JR 0100";
	static char const s9[] PROGMEM = "JP 0200";
	static char const s10[] PROGMEM = "JP 0400";
	static char const s11[] PROGMEM = "JP 0800";
	static char const s12[] PROGMEM = "JP 1000";
	static char const s13[] PROGMEM = "JP 2000";
	static char const s14[] PROGMEM = "JP 4000";
	static char const s15[] PROGMEM = "JP 8000";
	static char const s16[] PROGMEM = "Testing non-maskable interrupts";
	static char const s17[] PROGMEM = "LD SP, 1234";
	static char const s18[] PROGMEM = "Testing HALT mode";
	static char const s19[] PROGMEM = "HALT";
	static char const s20[] PROGMEM = "<-halt";
	static char const s21[] PROGMEM = "->int";
	static char const s22[] PROGMEM = "->nmi";
	static char const s23[] PROGMEM = "NOP (CPU must ignore this instruction)";
	static char const s24[] PROGMEM = "RETN";
	static char const s25[] PROGMEM = "Testing maskable interrupts";
	static char const s26[] PROGMEM = "IM 1";
	static char const s27[] PROGMEM = "EI";
	static char const s28[] PROGMEM = "Interrupt acknowledge";
	static char const s29[] PROGMEM = "RETI";
	static cpu_script_t const cpu_script[] PROGMEM = {
		{
			0x0000, _text, 0, s0,
		},
		// Testing line A0
		{
			0x0000, _m1, 0b00011000, s1,
		},
		{
			0x0001, _mrd, 0x00, NULL,
		},
		// A1
		{
			0x0002, _m1, 0b00011000, s2,
		},
		{
			0x0003, _mrd, 0x00, NULL,
		},
		// A2
		{
			0x0004, _m1, 0b00011000, s3,
		},
		{
			0x0005, _mrd, 0x02, NULL,
		},
		// A3
		{
			0x0008, _m1, 0b00011000, s4,
		},
		{
			0x0009, _mrd, 0x06, NULL,
		},
		// A4
		{
			0x0010, _m1, 0b00011000, s5,
		},
		{
			0x0011, _mrd, 0x0e, NULL,
		},
		// A5
		{
			0x0020, _m1, 0b00011000, s6,
		},
		{
			0x0021, _mrd, 0x1e, NULL,
		},
		// A6
		{
			0x0040, _m1, 0b00011000, s7,
		},
		{
			0x0041, _mrd, 0x3e, NULL,
		},
		// A7
		{
			0x0080, _m1, 0b00011000, s8,
		},
		{
			0x0081, _mrd, 0x7e, NULL,
		},
		// A8
		{
			0x0100, _m1, 0b11000011, s9,
		},
		{
			0x0101, _mrd, 0x00, NULL,
		},
		{
			0x0102, _mrd, 0x02, NULL,
		},
		// A9
		{
			0x0200, _m1, 0b11000011, s10,
		},
		{
			0x0201, _mrd, 0x00, NULL,
		},
		{
			0x0202, _mrd, 0x04, NULL,
		},
		// A10
		{
			0x0400, _m1, 0b11000011, s11,
		},
		{
			0x0401, _mrd, 0x00, NULL,
		},
		{
			0x0402, _mrd, 0x08, NULL,
		},
		// A11
		{
			0x0800, _m1, 0b11000011, s12,
		},
		{
			0x0801, _mrd, 0x00, NULL,
		},
		{
			0x0802, _mrd, 0x10, NULL,
		},
		// A12
		{
			0x1000, _m1, 0b11000011, s13,
		},
		{
			0x1001, _mrd, 0x00, NULL,
		},
		{
			0x1002, _mrd, 0x20, NULL,
		},
		// A13
		{
			0x2000, _m1, 0b11000011, s14,
		},
		{
			0x2001, _mrd, 0x00, NULL,
		},
		{
			0x2002, _mrd, 0x40, NULL,
		},
		// A14
		{
			0x4000, _m1, 0b11000011, s15,
		},
		{
			0x4001, _mrd, 0x00, NULL,
		},
		{
			0x4002, _mrd, 0x80, NULL,
		},
		// A15
		{
			0x8000, _text, 0, s16,
		},
		{
			0x8000, _m1, 0b00110001, s17,
		},
		{
			0x8001, _mrd, 0x34, NULL, // Lower 8 bits
		},
		{
			0x8002, _mrd, 0x12, NULL, // Upper 8 bits
		},
		{
			0x8003, _text, 0, s18,
		},
		{
			0x8003, _m1_halt, 0b01110110, s19,
		},
		{
			0x8004, _halt, 0, s20, // Check if in HALT
		},
		{
			0x8004, _int, 0, s21, // Assert interrupt
		},
		{
			0x8004, _halt, 0, s20, // Still in HALT
		},
		{
			0x8004, _nmi, 0, s22, // Assert NMI
		},
		{
			0x8004, _m1, 0b00000000, s23,
		},
		{
			0x8004, _m1, 0b00000000, s23,
		},
		{
			0x1233, _mwr, 0x80, NULL, // Push upper half of PC
		},
		{
			0x1232, _mwr, 0x04, NULL, // Push lower half of PC
		},
		{
			0x0066, _noint, 0, NULL,
		},
		{
			0x0066, _m1, 0b11101101, s24,
		},
		{
			0x0067, _mrd, 0b01000101, NULL,
		},
		{
			0x1232, _mrd, 0x04, NULL, // PC lower half
		},
		{
			0x1233, _mrd, 0x80, NULL, // PC upper half
		},
		{
			0x8004, _text, 0, s25,
		},
		{
			0x8004, _m1, 0b11101101, s26,
		},
		{
			0x8005, _mrd, 0b01010110, NULL,
		},
		{
			0x8006, _m1, 0b11111011, s27,
		},
		{
			0x8007, _m1, 0b01110110, s19,
		},
		{
			0x8008, _halt, 0, s20, // In HALT mode
		},
		{
			0x8008, _int, 0, s21, // Assert interrupt
		},
		{
			0x8008, _m1, 0b00000000, s23,
		},
		{
			0x8008, _intack, 0b00000000, s28,
		},
		{
			0x1233, _noint, 0, NULL,
		},
		{
			0x1233, _mwr, 0x80, NULL, // Push upper half of PC
		},
		{
			0x1232, _mwr, 0x08, NULL, // Push lower half of PC
		},
		{
			0x0038, _m1, 0b11101101, s29,
		},
		{
			0x0039, _mrd, 0b01001101, NULL,
		},
		{
			0x1232, _mrd, 0x08, NULL,
		},
		{
			0x1233, _mrd, 0x80, NULL,
		},
		{
			0x8008, _done, 0, NULL,
		},
	};

	// Begin by resetting the system and disable the internal memory and I/O mapping
	DDRG  |= 0b00000001;
	PORTG &= 0b11111110;
	
	DDRJ  |= 0b00001100;
	PORTJ &= 0b11110011;
	
	// Enable WAIT generator
	PORTB |= 0b00000001;
	
	// Make sure BUSRQ is not set
	DDRH &= 0b11110111;
	
	delay( 1000 );
	
	// Release RESET
	DDRG &= 0b11111110;

	if( !WaitAccess() )
	{
		pass = false;
	}
	
	for( tidx = cpu_script;
	     pass && 
		 ( memcpy_P( &script_entry, (void * const )tidx, sizeof( script_entry ) ),
		   script_entry.state != _done );
		 tidx++ )
	{
		if( ( (PINC << 8) | PINA ) != script_entry.addr )
		{
			printf_P( PSTR( "Processor has wrong address: %04x\nExpecting: %04x\n" ), (PINC << 8) | PINA, script_entry.addr );
			pass = false;
			break;
		}

		if( script_entry.info )
		{
			printf_P( PSTR( "%S\n" ), script_entry.info );
		}
		switch( script_entry.state )
		{
			case _text:
			continue;
			case _m1:
			case _m1_halt:
			case _intack:
			if( ( PINJ & 0b00000010 ) != 0 )
			{
				printf_P( PSTR( "Processor not asserting M1\n" ) );
				pass = false;
			}
			if( script_entry.state == _intack )
			{
				if( ( PINH & 0b00010111 ) != 0b00000111 )
				{
					printf_P( PSTR( "Processor is not acknowledging INT as expected\n" ) );
					pass = false;
				}
				break;
			}
			// Fall through
			case _mrd:
			if( ( PINH & 0b00010111 ) != 0b10001 )
			{
				printf_P( PSTR( "Processor is not reading memory as expected\n" ) );
				pass = false;
			}
			DDRD = 0b11111111;
			PORTD = script_entry.data;
			// After a HALT instruction, no memory access is expected
			if( script_entry.state == _m1_halt )
			{
				// But still, undo the WAIT state
				PORTB &= 0b11111110;
				PORTB |= 0b00000001;
				continue;
			}
			break;
			case _mwr:
			if( ( PINH & 0b00010111 ) != 0b10010 )
			{
				printf_P( PSTR( "Processor is not writing memory as expected\n" ) );
				pass = false;
			}
			else if( PIND != script_entry.data )
			{
				printf_P( PSTR( "Processor writing wrong data\n" ) );
				pass = false;
			}
			break;
			case _ird:
			if( ( PINH & 0b00010111 ) != 0b00000101 )
			{
				printf_P( PSTR( "Processor is not reading IO as expected\n" ) );
				pass = false;
			}
			DDRD = 0b11111111;
			PORTD = script_entry.data;
			break;
			case _iwr:
			if( ( PINH & 0b00010111 ) != 0b00110 )
			{
				printf_P( PSTR( "Processor is not writing IO as expected\n" ) );
				pass = false;
			}
			else if( PIND != script_entry.data )
			{
				printf_P( PSTR( "Processor writing wrong data\n" ) );
				pass = false;
			}
			break;
			case _int:
			DDRE |= 0b00000100;
			for( uint8_t i = 0; i < 0xff; i++ ) { _NOP(); }
			continue;
			case _nmi:
			DDRE |= 0b00001000;
			for( uint8_t i = 0; i < 0xff; i++ ) { _NOP(); }
			continue;
			case _noint:
			// Stop generating interrupts
			DDRE &= 0b11110011;
			continue;
			case _halt:
			if( ( PINB & 0b00010000 ) != 0 )
			{
				printf_P( PSTR( "Processor not in halt state as expected\n" ) );
				pass = false;
			}
			continue;
			case _done:
			break;
		}
		
		if( !WaitNextAccess() )
		{
			pass = false;
		}
	}

    if( pass )
	{
		printf_P( PSTR( "All CPU tests passed\n" ) );
	}

	// Release data bus
	DDRD = 0b00000000;

	// Reset processor again
	DDRG |= 0b00000001;

	// Release control lines
	DDRE &= 0b11110011;

	// Restore I/O and memory map
	DDRJ  &= 0b11110011;
	
	// Release WAIT
	DDRG &= 0b11011111;
	PORTB &= 0b11111110;
	PORTB |= 0b00010000;


	for( int i = 0; i < 1000; i++ ) { _NOP(); }

	// Release RESET
	DDRG &= 0b11111110;

	return pass;
}
